#include "SbotFocPlugin.h"

#include <cmath>

SbotFocPlugin::SbotFocPlugin(QObject* parent) {
    //初始化插件信息
    this->pluginInfo.pluginName = PLUGIN_NAME;
    this->pluginInfo.pluginQmlRes = PLUGIN_QMLRES;
    this->pluginInfo.pluginLogo = QIcon(":/Res/Icon/SbotFocPluginLogo.png");

    this->devPort.moveToThread(this);

    this->sbot_foc_packet = sbot_protocol_packet_create();

    connect(&this->timerSendToUI, &QTimer::timeout, this, &SbotFocPlugin::sendDataToui);
    connect(&this->devPort, &QSerialPort::readyRead, this, &SbotFocPlugin::doDataReciveWork);

    //分配空间
    for (int i = 0; i < 8; i++) {
        this->chartData.append(0);
    }
}

void SbotFocPlugin::PluginInitAtThreadHook()
{

}

int SbotFocPlugin::PluginThreadLoop() {
    // // 自动查找设备
    // if (!this->devPort.isOpen()) {
    //     this->refreshDev();
    //     msleep(2000);
    // }

    this->mutexRxbuffer.lock();
    if (!this->rxDataBuffer.isEmpty()) {
        int icnt = 0;
        int head_index = 0;

        head_index = this->rxDataBuffer.indexOf(this->sbot_foc_packet->p_message[0], 0);

        if (this->rxDataBuffer.length() > (head_index + sbot_foc_packet->size_message)) {
            // 对比头部
            for (; icnt < this->sbot_foc_packet->size_header; icnt++) {
                if (this->rxDataBuffer[head_index + icnt] != this->sbot_foc_packet->p_message[icnt]) {
                    // 头部对比错误,删除错误数据
                    this->rxDataBuffer.remove(head_index, head_index + icnt);
                    goto _continue;
                }
            }

            // 取出数据
            memcpy(&sbot_foc_packet->p_message[0], &rxDataBuffer[head_index], sbot_foc_packet->size_message);
            // 弹出已经取出的数据
            this->rxDataBuffer.remove(0, head_index + sbot_foc_packet->size_message);
            this->sfPacketIsNew = true;
        }
    }
_continue:
    this->mutexRxbuffer.unlock();

    QApplication::processEvents();  // 处理所有挂起的事件，但不阻塞

    if (this->sfPacketIsNew) {
        this->processRawData();
        this->sfPacketIsNew = false;
    }

    msleep(10);

    return 0;
}

void SbotFocPlugin::refreshDev() {
    int icnt = 0;

    QObject* sbotFocPortList = qmlObj->findChild<QObject*>("sbotFocPortList");
    if (!sbotFocPortList) {
        qWarning() << "Failed to find sbotFocPortList object in QML";
        return;
    }

    //清除所有元素
    QMetaObject::invokeMethod(sbotFocPortList, "clearElement");

    foreach (const QSerialPortInfo& info, QSerialPortInfo::availablePorts()) {
        this->devPort.setPort(info);
        if (this->devPort.open(QIODevice::ReadWrite))  // 以读写方式打开串口
        {
            QVariantMap newElement;
            // newElement["name"] = "设备" + QString::number(icnt);
            newElement["name"] = info.portName();
            newElement["selected"] = false;

            QMetaObject::invokeMethod(sbotFocPortList, "addElement", Q_ARG(QVariant, QVariant::fromValue(newElement)));

            this->devPort.close();
            icnt++;
        }
    }
}

void SbotFocPlugin::openDev(QString portName) {
    if (this->devPort.isOpen())  // 如果串口打开，则关闭
    {
        this->devPort.clear();
        this->devPort.close();
    } else {
        // 设置串口信息
        this->devPort.setPortName(portName);
        this->devPort.setBaudRate(115200);
        this->devPort.setDataBits((QSerialPort::DataBits)8);
        this->devPort.setStopBits((QSerialPort::StopBits)1);
        this->devPort.setParity(QSerialPort::NoParity);

        // 打开端口
        if (!this->devPort.open(QIODevice::ReadWrite)) {
            qDebug() << "open falid!";
            QMessageBox::warning(nullptr, tr("提示"), tr("设备打开失败！"), QMessageBox::Ok);
            return;
        }

        this->timerSendToUI.start(this->flushFreqData);  //打开定时器
    }
}

void SbotFocPlugin::terminalInput(const QString& text) {
    QByteArray data = text.toUtf8();

    data.append('\n');

    if (this->devPort.isOpen()) {
        this->devPort.write(data);
    } else {
        QMessageBox::warning(nullptr, tr("提示"), tr("未连接设备！"), QMessageBox::Ok);
    }
}

void SbotFocPlugin::sendDataToui() {
    this->timerSendToUI.stop();
    emit this->flushViewData(this->chartData);
    this->timestamp++;
    this->timerSendToUI.start(this->flushFreqData);
}

void SbotFocPlugin::doDataReciveWork() {
    // 丢掉数据保证实时性
    if (this->rxDataBuffer.length() > 512) {
        this->devPort.readAll();
        return;
    }

    this->mutexRxbuffer.lock();
    this->rxDataBuffer.append(devPort.readAll());
    this->mutexRxbuffer.unlock();
}

void SbotFocPlugin::processRawData() {
    float radianRtAngle = 0;
    float radianTarAngle = 0;
    float degreesRtAngle = 0;
    float degreesTarAngle = 0;
    float powerVolate = 0;

    float Ud = 0;
    float Uq = 0;

    float alphaValue = 0;
    float betaValue = 0;
    float aPhaseValue = 0;
    float bPhaseValue = 0;
    float cPhaseValue = 0;

    if (sbot_protocol_check_crc(this->sbot_foc_packet)) {
        sbot_protocol_get(sbot_foc_packet, SFRTANGLE, &radianRtAngle);
        degreesRtAngle = radianRtAngle * (180.0 / M_PI);

        sbot_protocol_get(sbot_foc_packet, SFTARANGLE, &radianTarAngle);
        degreesTarAngle = radianTarAngle * (180.0 / M_PI);
        degreesTarAngle = std::round(degreesTarAngle * 100.0) / 100.0;

        sbot_protocol_get(sbot_foc_packet, SFPOWERVOLTAGE, &powerVolate);
        powerVolate = std::round(powerVolate * 100.0) / 100.0;

        //解析数据
        Uq = powerVolate / 3;
        alphaValue = Ud * cos(degreesTarAngle) - Uq * sin(degreesTarAngle);
        betaValue = Uq * cos(degreesTarAngle) + Ud * sin(degreesTarAngle);
        alphaValue = std::round(alphaValue * 100.0) / 100.0;
        betaValue = std::round(betaValue * 100.0) / 100.0;

        aPhaseValue = alphaValue;
        bPhaseValue = (sqrt(3) * betaValue - alphaValue) / 2;
        cPhaseValue = (alphaValue - sqrt(3) * betaValue) / 2;
        aPhaseValue += powerVolate / 2;
        bPhaseValue += powerVolate / 2;
        cPhaseValue += powerVolate / 2;
        aPhaseValue = std::round(aPhaseValue * 100.0) / 100.0;
        bPhaseValue = std::round(bPhaseValue * 100.0) / 100.0;
        cPhaseValue = std::round(cPhaseValue * 100.0) / 100.0;

        this->chartData[0] = this->timestamp;
        this->chartData[1] = degreesRtAngle;
        this->chartData[2] = degreesTarAngle;
        this->chartData[3] = alphaValue;
        this->chartData[4] = betaValue;
        this->chartData[5] = aPhaseValue;
        this->chartData[6] = bPhaseValue;
        this->chartData[7] = cPhaseValue;
    }
}
